/*
  Copyright (c) 2010 Les Newell. All rights reserved

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
*/


#ifndef COMPASS_H_INCLUDED
#define COMPASS_H_INCLUDED

#include "multifilter.h"
#include "arctan2.h"
#include "math.h"

DEFINE_MODULE(Compass,Module)

public:
    Compass()
    {
        //heading in radians
        AddSocket("Compass.Heading", &m_heading);
    }


protected:
    SocketF m_heading;
};


DEFINE_MODULE(Compass3Axis,Compass)

public:
Compass3Axis()
{
    AddSocket("Compass3Axis.RawX", &m_axes[AXISX]);
    AddSocket("Compass3Axis.RawY", &m_axes[AXISY]);
    AddSocket("Compass3Axis.RawZ", &m_axes[AXISZ]);
    AddSocket("Compass3Axis.OffsetX", &m_offsets[AXISX]);
    AddSocket("Compass3Axis.OffsetY", &m_offsets[AXISY]);
    AddSocket("Compass3Axis.OffsetZ", &m_offsets[AXISZ]);
    AddSocket("Compass3Axis.Filter", m_filter.amounts);
    AddSocket("Compass3Axis.Rotation", &m_rotation);

}

virtual void Init()
{
    imuPitch.Connect("AHRS.pitch");
    imuRoll.Connect("AHRS.roll");
    if (!imuRoll.IsConnected()) //No IMU
    {
        imuPitch.Invalidate();
        Errorln("Compass: I need an AHRS");
    }
}

virtual void ParamChanged(Socket * param)
{
    if(param == &m_rotation)
    {
        while(m_rotation < 0)
        {
            m_rotation += 2 * M_PI;
        }
        while(m_rotation > 2 * M_PI)
        {
            m_rotation -= 2 * M_PI;
        }
        return;
    }
    m_filter.ParamChanged(param);
}


protected:
void CalculateHeading()
{
    if (!imuPitch.IsConnected()) //we haven't got an IMU so assume we are dead level
    {
        float ang = arctan2(m_axes[AXISX],m_axes[AXISY]);
        m_filter.Update(&ang);
        m_heading = m_filter.filtered[0];
        return;
    }
    float roll = imuRoll;
    float pitch = imuPitch;
    float xReading = m_axes[AXISX];
    float yReading = m_axes[AXISY];
    float zReading = m_axes[AXISZ];

    float cmX = (xReading * cos(pitch)) + (yReading *sin(roll) * sin(pitch)) + (zReading * cos(roll) * sin(pitch));
    float cmY = (yReading * cos(roll)) - (zReading * sin(roll));



    float ang = arctan2(cmX,cmY);
    m_filter.Update(&ang);
    m_heading = m_filter.filtered[0] + m_rotation;
    if(m_heading > M_PI)
    {
        m_heading -= 2 * M_PI;
    }
}


Socket16 m_axes[MAX_AXES];
Parameter16 m_offsets[MAX_AXES];
ParameterF m_rotation;

PlugF imuPitch;
PlugF imuRoll;
MultiFilterRef<1> m_filter;

};


#endif// #ifndef COMPASS_H_INCLUDED
